#include "protocol/modbus/ModbusTCP.h"
#include "core/Logger.h"
#include <iostream>
#include <thread>
#include <chrono>

int main(int argc, char* argv[]) {
    if (argc < 3) {
        std::cout << "Usage: " << argv[0] << " <host> <port> [slave_id] [address] [count]" << std::endl;
        std::cout << "Example: " << argv[0] << " 192.168.1.101 502 1 100 1" << std::endl;
        return 1;
    }

    std::string host = argv[1];
    int port = std::stoi(argv[2]);
    int slaveId = (argc > 3) ? std::stoi(argv[3]) : 1;
    int address = (argc > 4) ? std::stoi(argv[4]) : 100;
    int count = (argc > 5) ? std::stoi(argv[5]) : 1;

    std::cout << "Testing Modbus TCP connection:" << std::endl;
    std::cout << "Host: " << host << std::endl;
    std::cout << "Port: " << port << std::endl;
    std::cout << "Slave ID: " << slaveId << std::endl;
    std::cout << "Start Address: " << address << std::endl;
    std::cout << "Register Count: " << count << std::endl;
    std::cout << "----------------------------------------" << std::endl;

    try {
        // Create Modbus TCP client
        ems::protocol::ModbusTCP client(host, port);
        client.setSlaveId(slaveId);
        client.setTimeout(3000);

        std::cout << "Attempting to connect..." << std::endl;
        if (!client.connect()) {
            std::cout << "❌ Failed to connect: " << client.getLastError() << std::endl;
            return 1;
        }

        std::cout << "✅ Connected successfully!" << std::endl;

        // Test different function codes
        std::vector<std::pair<std::string, int>> tests = {
            {"Holding Registers (FC03)", 3},
            {"Input Registers (FC04)", 4},
            {"Coils (FC01)", 1},
            {"Discrete Inputs (FC02)", 2}
        };

        for (const auto& test : tests) {
            std::cout << "\nTesting " << test.first << ":" << std::endl;

            if (test.second == 3) {
                // Test holding registers
                std::vector<uint16_t> data;
                if (client.readHoldingRegisters(address, count, data)) {
                    std::cout << "✅ Success! Data: ";
                    for (size_t i = 0; i < data.size(); ++i) {
                        std::cout << data[i];
                        if (i < data.size() - 1) std::cout << ", ";
                    }
                    std::cout << std::endl;
                } else {
                    std::cout << "❌ Failed: " << client.getLastError() << std::endl;
                }
            } else if (test.second == 4) {
                // Test input registers
                std::vector<uint16_t> data;
                if (client.readInputRegisters(address, count, data)) {
                    std::cout << "✅ Success! Data: ";
                    for (size_t i = 0; i < data.size(); ++i) {
                        std::cout << data[i];
                        if (i < data.size() - 1) std::cout << ", ";
                    }
                    std::cout << std::endl;
                } else {
                    std::cout << "❌ Failed: " << client.getLastError() << std::endl;
                }
            } else if (test.second == 1) {
                // Test coils
                std::vector<bool> data;
                if (client.readCoils(address, count, data)) {
                    std::cout << "✅ Success! Data: ";
                    for (size_t i = 0; i < data.size(); ++i) {
                        std::cout << (data[i] ? "1" : "0");
                        if (i < data.size() - 1) std::cout << ", ";
                    }
                    std::cout << std::endl;
                } else {
                    std::cout << "❌ Failed: " << client.getLastError() << std::endl;
                }
            } else if (test.second == 2) {
                // Test discrete inputs
                std::vector<bool> data;
                if (client.readDiscreteInputs(address, count, data)) {
                    std::cout << "✅ Success! Data: ";
                    for (size_t i = 0; i < data.size(); ++i) {
                        std::cout << (data[i] ? "1" : "0");
                        if (i < data.size() - 1) std::cout << ", ";
                    }
                    std::cout << std::endl;
                } else {
                    std::cout << "❌ Failed: " << client.getLastError() << std::endl;
                }
            }

            // Small delay between tests
            std::this_thread::sleep_for(std::chrono::milliseconds(100));
        }

        client.disconnect();
        std::cout << "\n✅ Test completed successfully!" << std::endl;

    } catch (const std::exception& e) {
        std::cout << "❌ Exception: " << e.what() << std::endl;
        return 1;
    }

    return 0;
}